/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of 
*     its contributors may be used to endorse or promote products 
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Pedro Patron, Ioseba Tena Ruiz
*
*********************************************************************/

#ifndef _OSLCORE_GLOBAL_COORD_CONVERTER_H_
#define _OSLCORE_GLOBAL_COORD_CONVERTER_H_

#include <iostream>

#if defined(WIN32) || defined(_WIN32)
#else
using namespace std;
#endif

namespace osl_core {

#undef E_RAD_WGS84
#undef P_RAD_WGS84
#undef E_RAD
#undef P_RAD
#undef MAX_LATITUDE
#undef MIN_LATITUDE
#undef MAX_LONGITUDE
#undef MIN_LONGITUDE

/**
 * World Geodetic System 1984 Equatorial Radius
 * @see http://www.wgs84.com/
 */
#define E_RAD_WGS84 (double)	6378137.0

/**
 * World Geodetic System 1984 Polar Radius
 * @see http://www.wgs84.com/
 */
#define P_RAD_WGS84 (double)	6356752.3142

/**
 * Default Equatorial Radius
 */
#define E_RAD	E_RAD_WGS84

/**
 * Default Polar Radius
 */
#define P_RAD	P_RAD_WGS84


#define MAX_LATITUDE			90
#define MIN_LATITUDE			-90

#define MAX_LONGITUDE			180
#define	MIN_LONGITUDE			-180

/**
 * North,East,Depth coordinates
 */
typedef struct NED
{
	double n;		/*!< North */
	double e;		/*!< East */
	double d;		/*!< Depth */
} T_NED;

typedef struct dNED
{
	double dn;		/*!< North velocity */
	double de;		/*!< East velocity */
	double dd;		/*!< Depth velocity */
} T_dNED;

/**
 * East,North,Up coordinates
 */
typedef struct ENU
{
	double e;		/*!< East */
	double n;		/*!< North */
	double u;		/*!< Up */
} T_ENU;

/**
 * Latitude,Longitude,Depth coordinates stored in degres
 */
typedef struct LLD
{
	double lat;		/*!< Latitude */
	double lon;		/*!< Longitude */
	double depth;		/*!< Depth */
} T_LLD;

/**
 * Earth centred coordinates
 */
typedef struct ECC
{
	double i;		/*!< Earth centred coordinates I */
	double j;		/*!< Earth centred coordinates J */
	double k;		/*!< Earth centred coordinates K */
} T_ECC;

/**
 * Datum
 */
typedef struct datum
{
	double	a;			/*!< Equatorial Radius */
	double	b;			/*!< Polar Radius */
	double	i_f;		/*!< Inverse flattering */
	double	e;			/*!< Eccentricity */
	double	e_sq;		/*!< Eccentricity squared */
	double	se_sq;		/*!< Second Eccentricity squared */

	double	m0;
	double	m1;
	double	m2;
	double	m3;
	
} T_Datum;

#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, NED & ned);
#else
std::ostream & operator << (std::ostream & out, NED & ned);
#endif

#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, dNED & dned);
#else
std::ostream & operator << (std::ostream & out, dNED & dned);
#endif

#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, ENU & enu);
#else
std::ostream & operator << (std::ostream & out, ENU & enu);
#endif

#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, LLD & lld);
#else
std::ostream & operator << (std::ostream & out, LLD & lld);
#endif

#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, ECC & ecc);
#else
std::ostream & operator << (std::ostream & out, ECC & ecc);
#endif


T_NED operator +  (const T_NED & a, const T_NED & b);
T_NED operator -  (const T_NED & a, const T_NED & b);

// ===========================================================================
/**
 *	Coordinate systems converter
 *
 *	Converter between global coordinates. Coordinates converter.	
 * - Latitude,Longitude,Depth to Earth Centred Coordinates Converter and viceversa.
 * - Earth Centred to East,North,Up Coordinates Converter and viceversa.
 * - East,North,Up to North,East,Down Coordinats Converter and viceversa.
 *   
 */
class GlobalCoordConverter
{
	public:

		/**
		 * Constructor
		 * @param	equatRadius	Equatorial radius
		 * @param	polarRadius	Polar radius
		 *
		 * That constructor uses the Default Equatorial Radius E_RAD and the 
		 * Default Polar Radius P_RAD if no one of the parameters is used.
		 */
		GlobalCoordConverter (const double & equatRadius = E_RAD, const double & polarRadius = P_RAD);
		
		/**
		 * Destructor
		 */
		~GlobalCoordConverter ();

		/**
		 * @return true if the reference point has been initialised, false otherwise
		 */
		bool		isInitialised();

		/**
		 * Update the LLD reference point
		 * @param	originLLD	the new reference point
		 */
		void 		setLLDReference	(const T_LLD & originLLD);

		/**
		 * Update the LLD reference point by using a ECC point
		 * @param	originECC	the new reference point in ECC coordingates
		 */
		void	 	setECCReference (const T_ECC & originECC);
		
		/**
		 * Update the LLD reference point by using a ENU point
		 * @param	originENU	the new reference point in ENU coordinates
		 */
		void 		setENUReference (const T_ENU & originENU);
		
		/**
		 * Update the LLD reference point by using a NED point
		 * @param	originNED	the new reference point in NED coordinates
		 */
		void 		setNEDReference (const T_NED & originNED);

		/**
		 * @return the reference point in LLD coordinates
		 */
		T_LLD 		getLLDReference	(void);
		
		/**
		 * @return the reference point in ECC coordinates
		 */
		T_ECC 		getECCReference	(void);

		/**
		 * Converts North,East,Down to East,North,Up coordinates
		 * @param	ned	North,East,Down coordinates
		 * @return	point representation in East,North,Up coordinates
		 *
		 * \f$ned.n = enu.n \f$
		 * 
		 * \f$ned.e = enu.e \f$
		 * 
		 * \f$ned.d = -enu.u \f$
		 */
		T_ENU		NED2ENU (const	T_NED &	ned ) ;

		/**
		 * Converts East,North,Up to North,East,Down coordinates
		 * @param	enu	East,North,Up coordinates
		 * @return	point representation in North,East,Down coordinates
		 *
		 * \f$enu.e = ned.e \f$
		 * 
		 * \f$enu.n = ned.n \f$
		 * 
		 * \f$enu.u = -ned.d \f$
		 *
		 */
		T_NED 	 	ENU2NED (const	T_ENU &	enu ) ;
		
		/**
		 * 
		 * Converts East,North,Up to Earth Centred coordinates
		 * @param	enu	East,North,Up coordinates
		 * @return	point representation in Earth Centred coordinates
		 */
		T_ECC 	 	ENU2ECC (const	T_ENU &	enu ) ;
		
		/**
		 * Converts Earth Centred to East,North,Up coordinates
		 * @param	ecc	Earth Centred coordinates
		 * @return	point representation in East,North,Up coordinates
		 */
		T_ENU 	 	ECC2ENU (const	T_ECC &	ecc ) ;
	
		/**
		 * Converts Earth Centred to Latitude,Longitude,Depth coordinates
		 * @param	ecc	Earth Centred coordinates
		 * @return	point representation in Latitude,Longitude,Depth coordinates
		 */
		T_LLD 		ECC2LLD	(const	T_ECC &	ecc ) ;
		
		/**
		 * Converts Latitude,Longitude,Down to Earth Centred coordinates
		 * @param	lld	Latitude,Longitude,Down coordinates
		 * @return	point representation in Earth Centred coordinates
		 *
		 * \f$lld = lld*PI/180.0\f$
		 * 
		 * \f$x\_mer\_c = lld.depth + \frac{E\_RAD^2}{\sqrt{E\_RAD^2*\cos(lld.lat)^2 + P\_RAD^2*\sin(lld.lat)^2}}\f$
		 *
		 * \f$z\_c = lld.depth + \frac{P\_RAD^2}{\sqrt{E\_RAD^2*\cos(lld.lat)^2 + P\_RAD^2*\sin(lld.lat)^2}}\f$
		 * 
		 * \f$x\_mer = \cos(lld.lat)*x\_mer\_c\f$
		 * 
		 * \f$ecc.i = \cos(lld.lon)*x\_mer\f$
		 * 
		 * \f$ecc.j = \sin(lld.lon)*x\_mer\f$
		 * 
		 * \f$ecc.k = \sin(lld.lat)*z\_c\f$
		 *
		 */
		T_ECC 	 	LLD2ECC (const	T_LLD &	lld ) ;

		
		/**
		 * Converts East,North,Up to Latitude,Longitude,Down coordinates
		 * @param	enu	East,North,Up coordinates
		 * @return	point representation in Latitude,Longitude,Down coordinates
		 */
		T_LLD 	 	ENU2LLD (const	T_ENU &	enu ) ;		

		
		/**
		 * Converts Latitude,Longitude,Down to East,North,Up coordinates
		 * @param	lld	Latitude,Longitude,Down coordinates
		 * @return	point representation in East,North,Up coordinates
		 */
		T_ENU 	 	LLD2ENU (const	T_LLD &	lld ) ;

		
		/**
		 * Converts North,East,Down to Latitude,Longitude,Down coordinates
		 * @param	ned	North,East,Down coordinates
		 * @return	point representation in Latitude,Longitude,Down coordinates
		 */
		T_LLD 	 	NED2LLD (const	T_NED &	ned ) ;		

		
		/**
		 * Converts Latitude,Longitude,Down to North,East,Down coordinates
		 * @param	lld	Latitude,Longitude,Down coordinates
		 * @return	point representation in North,East,Down coordinates
		 */
		T_NED 	 	LLD2NED (const	T_LLD &	lld ) ;

	private:

		/** 
		 * Datum
		 */	
		T_Datum				m_datum;

		/**
		 * Reference in Longitude,Latitude coordinates
		 */
		T_LLD				m_LLDRef;		
};

} // namespace

#endif
